#pragma config(Hubs,  *S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     BL,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     BR,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     FR,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     FL,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     Arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     Lift,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    Wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

void Forward (int power, int duration)
{
	motor[FL] = power;
	motor[FR] = power;
	motor[BL] = power;
	motor[BR] = power;
	wait1Msec(duration);
}

void Backward (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = -1*power;
	motor[BL] = -1*power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void turnL (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = power;
	motor[BL] = power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void turnR (int power, int duration)
{
	motor[FL] = power;
	motor[FR] = -1*power;
	motor[BL] = -1*power;
	motor[BR] = power;
	wait1Msec(duration);
}

void driftL (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = power;
	motor[BL] = -1*power;
	motor[BR] = power;
	wait1Msec(duration);
}

void driftR (int power, int duration)
{
	motor[FL] = power;
	motor[FR] = -1*power;
	motor[BL] = power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void NE (int power, int duration)
{
	motor[FL] = power;
	motor[BR] = -1*power;
	motor[FR] = 0;
	motor[BL] = 0;
	wait1Msec(duration);
}

void SE (int power, int duration)
{
	motor[FL] = 0;
	motor[BR] = 0;
	motor[FR] = -1*power;
	motor[BL] = power;
	wait1Msec(duration);
}

void SW (int power, int duration)
{
	motor[FL] = -1*power;
	motor[BR] = power;
	motor[FR] = 0;
	motor[BL] = 0;
	wait1Msec(duration);
}

void NW (int power, int duration)
{
	motor[FL] = 0;
	motor[BR] = 0;
	motor[FR] = power;
	motor[BL] = -1*power;
	wait1Msec(duration);
}

void initializeRobot ()
{
	motor[FL] = 0;
	motor[BR] = 0;
	motor[FR] = 0;
	motor[BL] = 0;
	wait1Msec(1000);
}

task main()
{
	initializeRobot();
	while (true)
	{
		getJoystickSettings(joystick);

		if(joy1Btn(8))
		{
			driftR(50,25);
		}

		else if (joy1Btn(7))
		{
			driftL(50,25);
		}

		if(joystick.joy1_TopHat == 0)
		{
			Forward(50,25);			// FORWARD
		}

		else if(joystick.joy1_TopHat == 2)
		{
			turnR(25,25);				// TURN RIGHT
		}

		else if(joystick.joy1_TopHat == 4)
		{
			Backward(50,25);		// BACKWARD
		}

		else if(joystick.joy1_TopHat == 6)
		{
			turnL(25,25);				// TURN LEFT
		}
			if(joy1Btn(2))			  // Diagonal Southwest
		{
			SW(50,25);
		}
		else if(joy1Btn(3))   // Diagonal Southeast
		{
			SE(50,25);
		}
		else if(joy1Btn(4))   // Diagonal Northeast
		{
			NE(50,25);
		}
		else if(joy1Btn(1))   // Diagonal Northwest
		{
			NW(50,25);
		}
		else
		{
			motor[FL] = 0;
			motor[FR] = 0;
			motor[BL] = 0;
			motor[BR] = 0;
		}
	}
}
